program program_psr
use madx_ptc_module
use pointer_lattice
implicit none

    interface
       subroutine build_PSR(PSR,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: PSR
         logical(lp) mis
       end subroutine build_PSR
    end interface

type(layout), pointer:: psr
type(damap) id,m,a_l,dri
type(real_8) ray(6)
real(dp) closed_orbit(6),p(2),a11,a12,phase_advance(2),s
type(internal_state) state
type(normalform) normal
integer :: no1=0,np1=0 ,ind(4)=0,i,k,ph,mf
type(taylor) average_floquet,average_xp
logical(lp) mis
type(fibre), pointer :: f
real(dp), allocatable :: phad(:,:,:),pos(:)



call ptc_ini

psr=>m_u%start


Write(6,*) " small misalignments and rotations in lattice ? input t or f "
read(5,*) mis

write(6,*) " Usual Phase Advance (A_12=0)"
write(6,*) " and the bizarre Phase Advance (A_21=0)  "
write(6,*) "are compared. "
  
call build_PSR(PSR,mis)

allocate(phad(0:1,0:psr%n,2),pos(0:psr%n))
phad=0.d0
pos=0.d0

!!!  Phase space of Taylor maps will be (x,px,y,py) with delta dependence
!!! time is ignored in FPP but computed in PTC as ray(6)

state=only_4d0 

!do while(no1<=0) 
! write(6,*) " input an order > 0 (not too big first)! "
! read(5,*) no1
!enddo
no1=1

np1=0    !!!  No system parameters in the map

call init(state,no1,np1) ! PTC and FPP are properly initialized

write(6,*) " number of degrees of freedom ",c_%nd
write(6,*) 

call alloc(id,m,a_l,dri)  
call alloc(ray)
call alloc(normal)
call alloc(average_floquet,average_xp)

do ph=0,1

closed_orbit=0.d0 ! initial guess for closed orbit

call find_orbit(psr,closed_orbit,1,state,1.d-5)

write(6,'(a16,4(1x,g12.5))') " closed orbit = ",closed_orbit(1:4)

id=1;   ray=closed_orbit+id;   ! ray= closed orbit + identity map

call track(psr,ray,1,state) ! One turn map is computed via the ray

!!! The ray is really the result of a truncated power series algebra
!!! The map we are interested to compute is around the closed orbit: 
!!  it is really part of a diffential algebra
m=ray   !   The ray is "officially" turned into a "damap"  (DA in Berz's honour)

normal = m  ! The map is normalised   

a_l=normal%a_t

!!!!  Algorithm to force the linear A into the !!!!
!!!!  form A_12=A_34=0                         !!!!
      call purify(a_l,p,ph)
!!!!          End of the Algorithm             !!!!
 
ray=closed_orbit+a_l  ! Put the new A into RAY(6)

f=>psr%start

phase_advance=0.d0
s=0.d0

pos(0)=0.d0
phad(ph,0,1:2)=0.d0

do k=1,psr%n

 ray=a_l+closed_orbit

 call track(psr,ray,k,k+1,state)

 a_l=ray

!!!!  Algorithm to force the linear A into the !!!!
!!!!  form A_12=A_34=0  if ph=0                !!!!
!!!!  form A_21=A_43=0  if ph=1                !!!!
      call purify(a_l,p,ph)
!!!!          End of the Algorithm             !!!!   
 
    closed_orbit=ray

    phase_advance=phase_advance+p


    s=s+f%mag%p%ld    
    pos(k)=s
    phad(ph,k,1:2)=phase_advance/twopi
 
f=>f%next
enddo

write(6,*) 
write(6,*) " Fractional Tunes in both planes from Normal Form"
write(6,*) normal%tune(1:2)
write(6,*) 
write(6,*) " Phase advances in both planes "
write(6,*) phase_advance/twopi


enddo


 call kanalnummer(mf,"phase_difference.dat")

 write(mf,'(a40)')  "     s           Dphi_x      Dphi_y     "
  do k=0,psr%n
    p(1)=(phad(0,k,1)-phad(1,k,1) )
    p(2)=(phad(0,k,2)-phad(1,k,2))
    do while (p(1)>=0) 
     p(1)=p(1)-1 
    enddo
    do while (p(2)>=0) 
     p(2)=p(2)-1 
    enddo
    if(p(1)<-.5d0) p(1)=p(1)+1 ; if(p(2)<-.5d0)  p(2)=p(2)+1;
    if(mod(k,7)==0) then
     write(mf,'(3(1x,g12.5),a37)') pos(k),p," <-- symmetry point in ideal lattice "  
    else
     write(mf,'(3(1x,g12.5))') pos(k),p  
    endif 
  enddo

close(mf)

call kill(id,m,a_l,dri)  
call kill(ray)
call kill(normal)
call kill(average_floquet,average_xp)

deallocate(pos,phad)

write(6,*) "   "
write(6,*) " hit return to terminate program "
write(6,*) "   "
pause 
 
end program program_psr

!=================================================================
subroutine  purify(a_l,p,f)
use madx_ptc_module
use pointer_lattice
implicit none
real(dp), intent(inout) :: p(2)
type(damap), intent(inout) :: a_l
type(damap) dri
real(dp) a11,a12,a21,a22
integer ind(4),i,f

call alloc(dri)
 

   do i=1,2
    if(f==0) then  ! Courant-Snyder-Teng-Edwards A_12=0
          ind=0; ind(2*i-1)=1;
          a11=a_l%v(2*i-1).sub.ind
          ind=0; ind(2*i)=1;
          a12=a_l%v(2*i-1).sub.ind
          p(i)=atan2(a12,a11) 
    else           ! Anti Courant-Snyder A_21=0 
          ind=0; ind(2*i-1)=1;
          a21=a_l%v(2*i).sub.ind
          ind=0; ind(2*i)=1;
          a22=a_l%v(2*i).sub.ind
          p(i)=atan2(-a21,a22) 
    endif
 
    dri%v(2*i-1) = cos(p(i))*(1.0_dp.mono.(2*i-1))-sin(p(i))*(1.0_dp.mono.(2*i))
    dri%v(2*i)   = cos(p(i))*(1.0_dp.mono.(2*i))+sin(p(i))*(1.0_dp.mono.(2*i-1))
  enddo

    a_l=a_l*dri

call kill(dri)
 
end subroutine  purify

!=================================================================
subroutine  build_PSR(PSR,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: PSR

real(dp) :: ang, brho, kd, kf, Larc,SG(6),cut
type(fibre) :: b, d1, d2, qd, qf
type(layout) :: cell
logical(lp) mis
!-----------------------------------

call make_states(.false.)
exact_model = .true.
default = default + nocavity  
call update_states
madlength = .false.

ang = (twopi * 36.d0 / 360.d0)
Larc = 2.54948d0
brho = 1.2d0 * (Larc / ang)
call set_mad(brho = brho, method = 2, step = 10)
madkind2 = drift_kick_drift

kf =  2.72d0 / brho
kd = -1.92d0 / brho

d1 = drift("D1", 2.28646d0)
d2 = drift("D2", 0.45d0)
qf = quadrupole("QF", 0.5d0, kf)
qd = quadrupole("QD", 0.5d0, kd)
b  = rbend("B", Larc, ang)
cell = d1 + qd + d2 + b + d2 + qf + d1

PSR = 10 * cell
PSR = .ring.PSR

call survey(PSR)

if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(PSR,SIG,cut)
endif
end subroutine build_PSR
